#include "cPath.h"

cPath::cPath(void)
{
	status = STOP;

	x = -1;
	y = -1;

	t = 0.0f;
	stepLenght = STEP_LENGTH;
	speed=1.0f;
}

cPath::~cPath(void)
{
}

int cPath::Init(vector< void* >* path)
{
	if(!path)
		return 0;

	this->path = path;

	this->nNodes = path->size();

	if(this->nNodes < 2)
		return 0;

	t = 0.0f;

	//
	// Setup the initial indexes
	//
	i0 = -1;
	i1 = 0;
	i2 = 1;
	i3 = 2;

	SetCurrentNodes();

	return nNodes;
}

void cPath::Done()
{
	status=STOP;
}

bool cPath::IsDone()
{
	return status == STOP;
}

int cPath::NextNode()
{
	i0++;
	i1++;
	i2++;
	i3++;

	if(i2 == nNodes)
		return ARRIVE;

	SetCurrentNodes();

	return CONTINUE;
}

void cPath::SetStepLength(float l)
{
	this->stepLenght = l;
}

void cPath::SetSpeed(float s)
{
	this->speed = s;
}

int cPath::NextStep(int *x, int *y)
{
	if(t>=1.0f)
	{
		t = t - 1.0f;
		status = NextNode();
	}
	else
	{
		status = CONTINUE;
	}

	if(status == ARRIVE)
	{
		cNode *last = (cNode*)path->at(nNodes - 1);
		//
		// Last one!
		//
		*x = (int)last->x;
		*y = (int)last->y;
	}
	else
	{
		sTrackPoint out;

		sTrackPoint tp0 = {(float)(p0->x),(float)(p0->y)};
		sTrackPoint tp1 = {(float)(p1->x),(float)(p1->y)};
		sTrackPoint tp2 = {(float)(p2->x),(float)(p2->y)};
		sTrackPoint tp3 = {(float)(p3->x),(float)(p3->y)};

		PointOnCurve(out,t,tp0,tp1,tp2,tp3);
	
		*x = (int)out.x;
		*y = (int)out.y;

		//if((x == tp2.x) && (y == tp2.y))
		//return ARRIVE

		t = t+stepLenght;
		//status = CONTINUE;
	}

	return status;
}

// Catmull-Rom
void cPath::PointOnCurve(sTrackPoint& out, float t, sTrackPoint& p0, sTrackPoint& p1, sTrackPoint& p2, sTrackPoint& p3)
{
float t2 = t * t;
float t3 = t2 * t;
out.x = 0.5f * ( ( 2.0f * p1.x ) +
( -p0.x + p2.x ) * t +
( 2.0f * p0.x - 5.0f * p1.x + 4 * p2.x - p3.x ) * t2 +
( -p0.x + 3.0f * p1.x - 3.0f * p2.x + p3.x ) * t3 );
out.y = 0.5f * ( ( 2.0f * p1.y ) +
( -p0.y + p2.y ) * t +
( 2.0f * p0.y - 5.0f * p1.y + 4 * p2.y - p3.y ) * t2 +
( -p0.y + 3.0f * p1.y - 3.0f * p2.y + p3.y ) * t3 );
}

void cPath::SetCurrentNodes()
{
	// if previous node is not present, use current one
	if(i0 > 0)
		p0 = (cNode*)path->at(i0);
	else
		p0 = (cNode*)path->at(0);
	
	p1 = (cNode*)path->at(i1);
	p2 = (cNode*)path->at(i2);

	printf("\nFrom %s to %s, distance %f:\n",p1->id.c_str(),p2->id.c_str(),Distance(*p1,*p2));

	if(i3 == this->nNodes)
		p3 = (cNode*)path->at(nNodes - 1);
	else
		p3 = (cNode*)path->at(i3);

	//
	// Try to compute a step based on the enemy speed (right now the result will not be constant) 
	//
	float distance = Distance(*p1,*p2);

	this->stepLenght = 1.0f / ((float)distance/(float)speed);
}

float cPath::Distance(cNode& n1, cNode& n2)
{
	int dx = n1.x - n2.x;
	int dy = n1.y - n2.y;
	
	return (float) sqrt( (double)(dx*dx) + (double)(dy*dy) );
}

float cPath::Cost(cNode &n1, cNode &n2)
{
	return 0.0f;
}